/*
 *  ExampleRoadmap.cpp
 *  RVO2 Library.
 *  
 *  
 *  Copyright (C) 2008-10 University of North Carolina at Chapel Hill.
 *  All rights reserved.
 *  
 *  Permission to use, copy, modify, and distribute this software and its
 *  documentation for educational, research, and non-profit purposes, without
 *  fee, and without a written agreement is hereby granted, provided that the
 *  above copyright notice, this paragraph, and the following four paragraphs
 *  appear in all copies.
 *  
 *  Permission to incorporate this software into commercial products may be
 *  obtained by contacting the University of North Carolina at Chapel Hill.
 *  
 *  This software program and documentation are copyrighted by the University of
 *  North Carolina at Chapel Hill. The software program and documentation are
 *  supplied "as is", without any accompanying services from the University of
 *  North Carolina at Chapel Hill or the authors. The University of North
 *  Carolina at Chapel Hill and the authors do not warrant that the operation of
 *  the program will be uninterrupted or error-free. The end-user understands
 *  that the program was developed for research purposes and is advised not to
 *  rely exclusively on the program for any reason.
 *  
 *  IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR ITS
 *  EMPLOYEES OR THE AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT,
 *  SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS,
 *  ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE
 *  UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED
 *  OF THE POSSIBILITY OF SUCH DAMAGE.
 *  
 *  THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY
 *  DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 *  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY
 *  STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS
 *  ON AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND
 *  THE AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES,
 *  ENHANCEMENTS, OR MODIFICATIONS.
 *  
 *  Please send all BUG REPORTS to:
 *  
 *  geom@cs.unc.edu
 *  
 *  The authors may be contacted via:
 *  
 *  Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, and
 *  Dinesh Manocha
 *  Dept. of Computer Science
 *  Frederick P. Brooks Jr. Computer Science Bldg.
 *  3175 University of N.C.
 *  Chapel Hill, N.C. 27599-3175
 *  United States of America
 *  
 *  http://gamma.cs.unc.edu/RVO2/
 *  
 */

/*
 * Example file showing a demo with 100 agents split in four groups initially
 * positioned in four corners of the environment. Each agent attempts to move to
 * other side of the environment through a narrow passage generated by four
 * obstacles. There is a roadmap to guide the agents around the obstacles.
 */


#include <cstdlib>
#include <ctime>
#include <iostream>
#include <map>
#include <vector>
#include <windows.h>
#include <gl/glut.h>
#if HAVE_OPENMP || _OPENMP
#include <omp.h>
#endif

#ifdef __APPLE__
#include <RVO/RVO.h>
#else
#include "RVO.h"
#endif

#ifndef M_PI
static const float M_PI = 3.14159265358979323846f;
#endif


struct RoadmapVertex {
  RVO::Vector2 position;
  std::vector<int> neighbors;
  std::vector<float> distToGoal;
};

/* Store the roadmap. */
std::vector<RoadmapVertex> roadmap;

/* Store the goals of the agents. */
std::vector<int> goals;
float curFPS=2000;
const int screenwidth=640;
const int screenheight=480;
RVO::RVOSimulator * sim=NULL;
void setupScenario(RVO::RVOSimulator* sim)
{
  /* Seed the random number generator. */
  std::srand(static_cast<unsigned int>(std::time(0)));

  /* Specify the global time step of the simulation. */
  sim->setTimeStep(0.25f);

  /*
   * Add (polygonal) obstacles, specifying their vertices in counterclockwise
   * order.
   */
  std::vector<RVO::Vector2> obstacle1, obstacle2, obstacle3, obstacle4;

  obstacle1.push_back(RVO::Vector2(-10.0f, 40.0f));
  obstacle1.push_back(RVO::Vector2(-40.0f, 40.0f));
  obstacle1.push_back(RVO::Vector2(-40.0f, 10.0f));
  obstacle1.push_back(RVO::Vector2(-10.0f, 10.0f));

  obstacle2.push_back(RVO::Vector2(10.0f, 40.0f));
  obstacle2.push_back(RVO::Vector2(10.0f, 10.0f));
  obstacle2.push_back(RVO::Vector2(40.0f, 10.0f));
  obstacle2.push_back(RVO::Vector2(40.0f, 40.0f));

  obstacle3.push_back(RVO::Vector2(10.0f, -40.0f));
  obstacle3.push_back(RVO::Vector2(40.0f, -40.0f));
  obstacle3.push_back(RVO::Vector2(40.0f, -10.0f));
  obstacle3.push_back(RVO::Vector2(10.0f, -10.0f));

  obstacle4.push_back(RVO::Vector2(-10.0f, -40.0f));
  obstacle4.push_back(RVO::Vector2(-10.0f, -10.0f));
  obstacle4.push_back(RVO::Vector2(-40.0f, -10.0f));
  obstacle4.push_back(RVO::Vector2(-40.0f, -40.0f));

  sim->addObstacle(obstacle1);
  sim->addObstacle(obstacle2);
  sim->addObstacle(obstacle3);
  sim->addObstacle(obstacle4);

  /* Process the obstacles so that they are accounted for in the simulation. */
  sim->processObstacles();

  /* Add roadmap vertices. */
  RoadmapVertex v;

  /* Add the goal positions of agents. */
  v.position = RVO::Vector2(-75.0f, -75.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(75.0f, -75.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(-75.0f, 75.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(75.0f, 75.0f);
  roadmap.push_back(v);

  /* Add roadmap vertices around the obstacles. */
  v.position = RVO::Vector2(-42.0f, -42.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(-42.0f, -8.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(-42.0f, 8.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(-42.0f, 42.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(-8.0f, -42.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(-8.0f, -8.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(-8.0f, 8.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(-8.0f, 42.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(8.0f, -42.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(8.0f, -8.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(8.0f, 8.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(8.0f, 42.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(42.0f, -42.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(42.0f, -8.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(42.0f, 8.0f);
  roadmap.push_back(v);
  v.position = RVO::Vector2(42.0f, 42.0f);
  roadmap.push_back(v);

  /* Specify the default parameters for agents that are subsequently added. */
  sim->setAgentDefaults(15.0f, 10, 5.0f, 5.0f, 2.0f, 2.0f);

  /*
   * Add agents, specifying their start position, and store goals on the
   * opposite side of the environment (roadmap vertices).
   */
  for (int i = 0; i < 5; ++i) {
    for (int j = 0; j < 5; ++j) {
      sim->addAgent(RVO::Vector2(55.0f + i * 10.0f,  55.0f + j * 10.0f));
      goals.push_back(0);

      sim->addAgent(RVO::Vector2(-55.0f - i * 10.0f,  55.0f + j * 10.0f));
      goals.push_back(1);

      sim->addAgent(RVO::Vector2(55.0f + i * 10.0f, -55.0f - j * 10.0f));
      goals.push_back(2);

      sim->addAgent(RVO::Vector2(-55.0f - i * 10.0f, -55.0f - j * 10.0f));
      goals.push_back(3);
    }
  }
}


void updateVisualization(RVO::RVOSimulator* sim)
{
  /* Output the current global time. */
  std::cout << sim->getGlobalTime() << " ";

  /* Output the current position of all the agents. */
  glBegin(GL_POINTS);	
  for(int i=0;i<sim->getNumObstacleVertices();i++)
  {
	  glVertex2f(sim->getObstacleVertex(i).x(),sim->getObstacleVertex(i).y());
//	  glVertex2f(sim->getObstaclePoint2(i).x(),sim->getObstaclePoint2(i).y());
  }
  glEnd();
  glBegin(GL_POINTS);
  for (int i = 0; i < sim->getNumAgents(); ++i) {
	  //std::cout << sim->getAgentPosition( i ) << " " << sim->getAgentOrientation( i ) << " ";
	  glVertex2f(sim->getAgentPosition(i).x(),sim->getAgentPosition(i).y());
  }
  glEnd();
  
//   glBegin(GL_POINTS);
//   for (int i = 0; i < sim->getNumRoadmapVertices(); ++i) {
// 	  //  std::cout << sim->getAgentPosition( i ) << " " << sim->getAgentOrientation( i ) << " ";
// 	  glVertex2f(sim->getRoadmapVertexPosition(i).x(),sim->getRoadmapVertexPosition(i).y());
//   }
//   glEnd(}
}

void buildRoadmap(RVO::RVOSimulator* sim)
{
  /* Connect the roadmap vertices by edges if mutually visible. */
#pragma omp parallel for
  for (int i = 0; i < static_cast<int>(roadmap.size()); ++i) {
    for (int j = 0; j < static_cast<int>(roadmap.size()); ++j) {
      if (sim->queryVisibility(roadmap[i].position, roadmap[j].position, sim->getAgentRadius(0))) {
        roadmap[i].neighbors.push_back(j);
      }
    }

    /* 
     * Initialize the distance to each of the four goal vertices at infinity
     * (9e9f).
     */
    roadmap[i].distToGoal.resize(4, 9e9f);
  }

  /*
   * Compute the distance to each of the four goals (the first four vertices)
   * for all vertices using Dijkstra's algorithm.
   */
#pragma omp parallel for
  for (int i = 0; i < 4; ++i) {
    std::multimap<float, int> Q;
    std::vector<std::multimap<float, int>::iterator> posInQ(roadmap.size(), Q.end());

    roadmap[i].distToGoal[i] = 0.0f;
    posInQ[i] = Q.insert(std::make_pair(0.0f, i));

    while (!Q.empty()) {
      const int u = Q.begin()->second;
      Q.erase(Q.begin());
      posInQ[u] = Q.end();

      for (int j = 0; j < static_cast<int>(roadmap[u].neighbors.size()); ++j) {
        const int v = roadmap[u].neighbors[j];
        const float dist_uv = RVO::abs(roadmap[v].position - roadmap[u].position);

        if (roadmap[v].distToGoal[i] > roadmap[u].distToGoal[i] + dist_uv) {
          roadmap[v].distToGoal[i] = roadmap[u].distToGoal[i] + dist_uv;

          if (posInQ[v] == Q.end()) {
            posInQ[v] = Q.insert(std::make_pair(roadmap[v].distToGoal[i], v));
          } else {
            Q.erase(posInQ[v]);
            posInQ[v] = Q.insert(std::make_pair(roadmap[v].distToGoal[i], v));
          }
        }
      }
    }
  }

}

void setPreferredVelocities(RVO::RVOSimulator* sim)
{
  /*
   * Set the preferred velocity to be a vector of unit magnitude (speed) in the
   * direction of the visible roadmap vertex that is on the shortest path to the
   * goal.
   */
#pragma omp parallel for
  for (int i = 0; i < static_cast<int>(sim->getNumAgents()); ++i) {
    float minDist = 9e9f;
    int minVertex = -1;

    for (int j = 0; j < static_cast<int>(roadmap.size()); ++j) {
      if (RVO::abs(roadmap[j].position - sim->getAgentPosition(i)) + roadmap[j].distToGoal[goals[i]] < minDist &&
          sim->queryVisibility(sim->getAgentPosition(i), roadmap[j].position, sim->getAgentRadius(i))) {
          
        minDist = RVO::abs(roadmap[j].position - sim->getAgentPosition(i)) + roadmap[j].distToGoal[goals[i]];
        minVertex = j;
      }
    }

    if (minVertex == -1) {
      /* No roadmap vertex is visible; should not happen. */
      sim->setAgentPrefVelocity(i, RVO::Vector2(0, 0));
    } else {
      if (RVO::absSq(roadmap[minVertex].position -
        sim->getAgentPosition(i)) == 0.0f) {
          if (minVertex == goals[i]) {
            sim->setAgentPrefVelocity(i, RVO::Vector2());
          } else {
            sim->setAgentPrefVelocity(i, RVO::normalize(roadmap[goals[i]].position - sim->getAgentPosition(i)));
          }
      } else {
        sim->setAgentPrefVelocity(i, RVO::normalize(roadmap[minVertex].position - sim->getAgentPosition(i)));
      }
    }

    /*
     * Perturb a little to avoid deadlocks due to perfect symmetry.
     */
    float angle = std::rand() * 2.0f * M_PI / RAND_MAX;
    float dist = std::rand() * 0.0001f / RAND_MAX;

    sim->setAgentPrefVelocity(i, sim->getAgentPrefVelocity(i) + 
        dist * RVO::Vector2(std::cos(angle), std::sin(angle)));
  }
}

bool reachedGoal(RVO::RVOSimulator* sim)
{
  /* Check if all agents have reached their goals. */
  for (size_t i = 0; i < sim->getNumAgents(); ++i) {
    if (RVO::absSq(sim->getAgentPosition(i) - roadmap[goals[i]].position) > 20.0f * 20.0f) {
      return false;
    }
  }

  return true;
}

void getFPS()
{
	static int frame = 0, time, timebase = 0;
	static char buffer[256];

	frame++;
	time=glutGet(GLUT_ELAPSED_TIME);

	if (time - timebase > 1000) {
		curFPS=frame*1000.0/(time-timebase);
		sprintf(buffer,"FPS:%4.2f",
			curFPS);
		timebase = time;		
		frame = 0;
	}

	glutSetWindowTitle(buffer);
}
void display(int value)
{
	static int count=0;
	//cout<<count++<<"#";
	getFPS();
		glClear(GL_COLOR_BUFFER_BIT);
	if(reachedGoal(sim) ) return;
		updateVisualization(sim);
		setPreferredVelocities(sim);
		sim->doStep();

	glFlush();

	glutPostRedisplay();
	glutTimerFunc(20,display,0); 
}
void display0()
{
	getFPS();
	//	glClear(GL_COLOR_BUFFER_BIT);

	glFlush();
}
int init()
{
  /* Create a new simulator instance. */
  sim = new RVO::RVOSimulator();

  /* Set up the scenario. */
  setupScenario(sim);

  /* Build the roadmap. */
  buildRoadmap(sim);

  
  /* Perform (and manipulate) the simulation. */


  glClearColor(1,1,1,1);
  glColor3f(.0,0.0,0.0);

  glPointSize(2);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluOrtho2D(-(GLdouble)screenwidth/2,(GLdouble)screenwidth/2,-(GLdouble)screenheight/2,(GLdouble)screenheight/2);
  glClear(GL_COLOR_BUFFER_BIT);

  return 0;
}

int main(int argc,char ** argv)
{
	glutInit(&argc,argv);
	glutInitDisplayMode(GLUT_SINGLE|GLUT_RGB);
	glutInitWindowSize(screenwidth,screenheight);
	glutInitWindowPosition(100,150);
	glutCreateWindow("hello");
	glutTimerFunc(20,display,0); 
	glutDisplayFunc(display0);
	//glutIdleFunc(display);

	init();
	glutMainLoop();
}